#include<ros/ros.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include<std_msgs/Float64.h>

geometry_msgs::TwistStamped current_twist,last_twist;
void twist_cb_(const geometry_msgs::TwistStamped::ConstPtr &msg2)
{
    current_twist= *msg2;
}

geometry_msgs::Vector3Stamped cur_acc;
double last_time,cur_time,dt;
int flag=0;
int count=0;

int main(int argc, char** argv)
{
    ros::init(argc,argv,"acc_node");
    ros::NodeHandle ac_nh;

    ros::Subscriber  current_vel=ac_nh.subscribe<geometry_msgs::TwistStamped>
            ("/mavros/local_position/velocity_local",10,twist_cb_);

    ros::Publisher current_acc=ac_nh.advertise<geometry_msgs::Vector3Stamped>
            ("/current_accel",10);
  /*   ros::Publisher x_state=ac_nh.advertise<std_msgs::Float64>("/xdir_state",10);
    ros::Publisher y_state=ac_nh.advertise<std_msgs::Float64>("/ydir_state",10);
    ros::Publisher z_state=ac_nh.advertise<std_msgs::Float64>("/zdir_state",10); 
 */
    ros::Rate rate(20);

    while(ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
        if(flag==0)
        {
            last_time=ros::Time::now().toSec();
            last_twist=current_twist;
            flag=1;
        }
        if(flag==1)
        {
            cur_time=ros::Time::now().toSec();
            ROS_INFO("current time=%f",cur_time);
            dt=cur_time-last_time;
            cur_acc.header.stamp=ros::Time::now();
            cur_acc.header.frame_id="map";
            cur_acc.vector.x=(current_twist.twist.linear.x-last_twist.twist.linear.x)/dt;
            cur_acc.vector.y=(current_twist.twist.linear.y-last_twist.twist.linear.y)/dt;
            cur_acc.vector.z=(current_twist.twist.linear.z-last_twist.twist.linear.z)/dt;
            current_acc.publish(cur_acc);

            last_time=cur_time;
            last_twist=current_twist;

            ROS_INFO("acc.x=%.2f",cur_acc.vector.x);
            ROS_INFO("acc.y=%.2f",cur_acc.vector.y);
            ROS_INFO("acc.z=%.2f",cur_acc.vector.z);

            if(cur_acc.vector.x==0||cur_acc.vector.y==0||cur_acc.vector.z==0)
            {
                count++;
            }


            /*  std_msgs::Float64 temx,temy,temz;
            temx.data=cur_acc.vector.x;
            temy.data=cur_acc.vector.y;
            temz.data=cur_acc.vector.z;
            x_state.publish(temx);
            y_state.publish(temy);
            z_state.publish(temz);
            ROS_INFO("COUNT=%d",count);
 */

        }
    }
    return 0;


}